﻿using System;
using System.IO;
using cpp_class_lib;
using System.Timers;
using System.Windows.Forms;

namespace USMCdotNET
{
    public struct StartPRMSLine
    {
        Line l;
        int i;
        public StartPRMSLine(Line o, int ind)
        {
            l = o;
            i = ind;
        }
        public byte Step
        {
            get { return l.GetStepSet(i); }
            set { l.SetStepSet(value, i); }
        }
        public bool BacklashDir
        {
            get { return l.GetBacklashDir(i); }
            set { l.SetBacklashDir(value, i); }
        }
        public bool AutoBacklash
        {
            get { return l.GetAutoBacklash(i); }
            set { l.SetAutoBacklash(value, i); }
        }
        public bool SlowStart
        {
            get { return l.GetSlowStart(i); }
            set { l.SetSlowStart(value, i); }
        }
        public bool WaitInputSync
        {
            get { return l.GetWaitInputSync(i); }
            set { l.SetWaitInputSync(value, i); }
        }
        public bool OutSyncCounterRes
        {
            get { return l.GetOutSyncCounterRes(i); }
            set { l.SetOutSyncCounterRes(value, i); }
        }
        public bool ForceLoft
        {
            get { return l.GetForceLoft(i); }
            set { l.SetForceLoft(value, i); }
        }
    }
    public sealed class BaseLine :IPathElement
    {
        Line line;
        Vector<int> destination;
        public StartPRMSLine[] StPrms;
        Devices dev;
        int[] startpos;
        Vector<float> speed;
        public Vector<int> Destination { get { return destination; } set { destination = value; } }
        bool atProcess = false;
        bool finished = false;
        float velocity;
        public bool AtProcess
        {
            get { return atProcess; }
        }
        public bool Accomplished
        {
            get { return finished; }
        }
        public float Velocity
        {
            get { return velocity; }
            set
            {
                velocity = value;
                speed = new Vector<float>(destination.X, destination.Y, (a, b) => (a + b), (a, b) => (a * b), (a) => ((float)Math.Sqrt(a)));
                speed *= (velocity / speed.Length);
            }
        }
        public Vector<float> Speed //используется в прямоугольнике для коррекции скорости 
        {
            get { return speed; }
            set { speed = value; }
        }
        public void Save(BinaryWriter b)
        {
            b.Write("Baseline");
            b.Write(startpos[0]);
            b.Write(startpos[1]);
            b.Write(destination.X);
            b.Write(destination.Y);
            b.Write(speed.X);
            b.Write(speed.Y);
            b.Write("end_Baseline");

        }
        /*public void Load(BinaryReader a)
        {
            if (a.ReadString() == "Baseline")
            {
                startpos[0] = a.ReadInt32();
                startpos[1] = a.ReadInt32();
                destination.Change(a.ReadInt32(), a.ReadInt32());
                speed.X = a.ReadInt32();
                speed.Y = a.ReadInt32();
                a.ReadString();
            }
        }*/
        public void Move()
        {
            if (!finished && !atProcess)
            {
                /*      System.Timers.Timer tm = new System.Timers.Timer(5);
                      tm.Elapsed += (object s, ElapsedEventArgs e) =>
                      {
                          dev.GetSTATE();
                      };
                      tm.Start();
                      while (dev.STATE[0].RUN || dev.STATE[1].RUN)
                      { }
                      tm.Stop();
                      atProcess = true;
                      int res = line.Move();
                      if (res != 0)
                      {
                          dev.Stop();
                          throw new DeviceMovementException(this);
                      }
                      finished = false;
                      tm.Start();
                      while (!finished)
                      {
                          finished = !dev.STATE[0].RUN && !dev.STATE[1].RUN;
                      }
                      atProcess = false;
                      finished = true;*/
                atProcess = true;
                finished = false;
                int res = line.Move();
                if (res != 0)
                {
                    dev.Stop();
                    throw new DeviceMovementException(this);
                }
                System.Timers.Timer tm = new System.Timers.Timer(5);
                tm.Elapsed += (object s, ElapsedEventArgs e) =>
                {
                    dev.GetSTATE();
                };
                System.Threading.Thread.Sleep(5);
                dev.GetSTATE();
                tm.Start();
                while (!finished)
                {
                    finished = !dev.STATE[0].RUN && !dev.STATE[1].RUN;
                }
                tm.Stop();
                atProcess = false;
                finished = true;
            }
        }
        public void Return()
        {
            atProcess = true;
            Line temp = new Line(startpos[0], startpos[1], speed.X, speed.Y);
            int res = temp.Move();
            atProcess = false;
            finished = true;
            if (res != 0)
            {
                dev.Stop();
                throw new DeviceMovementException(this);
            }
        }
        public BaseLine(Vector<int> startPos, Vector<int> vector, float sp, StartPRMSLine[] a, Devices d)
        {
            this.destination = vector;
            this.velocity = sp;
            dev = d;
            this.speed = new Vector<float>(destination.X - startPos.X, destination.Y - startPos.Y, (c, b) => (c + b), (c, b) => (c * b), (c) => ((float)Math.Sqrt(c)));
            this.speed *= (velocity / this.speed.Length);
            line = new Line(destination.X, destination.Y, speed.X, speed.Y);
            StPrms = new StartPRMSLine[2];
            StPrms[0] = new StartPRMSLine(line, 0);
            StPrms[0].AutoBacklash = a[0].AutoBacklash;
            StPrms[0].BacklashDir = a[0].BacklashDir;
            StPrms[0].ForceLoft = a[0].ForceLoft;
            StPrms[0].OutSyncCounterRes = a[0].OutSyncCounterRes;
            StPrms[0].SlowStart = a[0].SlowStart;
            StPrms[0].Step = a[0].Step;
            StPrms[0].WaitInputSync = a[0].WaitInputSync;
            StPrms[1] = new StartPRMSLine(line, 1);
            StPrms[1].AutoBacklash = a[1].AutoBacklash;
            StPrms[1].BacklashDir = a[1].BacklashDir;
            StPrms[1].ForceLoft = a[1].ForceLoft;
            StPrms[1].OutSyncCounterRes = a[1].OutSyncCounterRes;
            StPrms[1].SlowStart = a[1].SlowStart;
            StPrms[1].Step = a[1].Step;
            StPrms[1].WaitInputSync = a[1].WaitInputSync;
            startpos = new int[2];
            startpos[0] = dev.STATE[0].CurPos;
            startpos[1] = dev.STATE[1].CurPos;
        }
        public BaseLine(int x, int y, float sp, StartPRMSLine[] a, Devices d)
        {
            if (sp > d.MaxSpeed)
                sp = d.MaxSpeed;
            dev = d;
            dev.GetSTATE();
            int x0 = dev.STATE[0].CurPos;
            int y0 = dev.STATE[1].CurPos;
            finished = (x0 == x && y0 == y);
            dev.GetMODE();
            Vector<int> vector = new Vector<int>(x, y, (c, b) => (c + b), (c, b) => (c * b), (c) => ((int)Math.Sqrt(c)));
            this.destination = vector;
            this.velocity = sp;
            this.speed = new Vector<float>(Math.Abs(x - x0), Math.Abs(y - y0), (c, b) => (c + b), (c, b) => (c * b), (c) => ((float)Math.Sqrt(c)));
            float length = speed.Length;
            if (length != 0)
                this.speed *= (velocity / length);
            else
            {
                this.speed.X = 0;
                this.speed.Y = 0;
            }
            line = new Line(destination.X, destination.Y, speed.X, speed.Y);
            StPrms = new StartPRMSLine[2];
            StPrms[0] = new StartPRMSLine(line, 0);
            StPrms[0].AutoBacklash = a[0].AutoBacklash;
            StPrms[0].BacklashDir = a[0].BacklashDir;
            StPrms[0].ForceLoft = a[0].ForceLoft;
            StPrms[0].OutSyncCounterRes = a[0].OutSyncCounterRes;
            StPrms[0].SlowStart = a[0].SlowStart;
            StPrms[0].Step = a[0].Step;
            StPrms[0].WaitInputSync = a[0].WaitInputSync;
            StPrms[1] = new StartPRMSLine(line, 1);
            StPrms[1].AutoBacklash = a[1].AutoBacklash;
            StPrms[1].BacklashDir = a[1].BacklashDir;
            StPrms[1].ForceLoft = a[1].ForceLoft;
            StPrms[1].OutSyncCounterRes = a[1].OutSyncCounterRes;
            StPrms[1].SlowStart = a[1].SlowStart;
            StPrms[1].Step = a[1].Step;
            StPrms[1].WaitInputSync = a[1].WaitInputSync;
            startpos = new int[2];
            startpos[0] = dev.STATE[0].CurPos;
            startpos[1] = dev.STATE[1].CurPos;
        }
        public StartPRMSLine FirstStPrms
        {
            get { return StPrms[0]; }
        }
        public StartPRMSLine SecondStPrms
        {
            get { return StPrms[1]; }
        }
        public int[] StartPos
        {
            get { return startpos; }
        }
    }
}
